/*
 * main.cpp
 *
 *  Created on: 17.11.2009
 *  Author: 	Martin Do
 *	Copyright: 	Martin Do, Chair Prof. Dillmann (IAIM),
 *            	Institute for Anthropomatics (IFA),
 *		        Karlsruhe Institute of Technology (KIT). All rights reserved.
 *
 *	Changes:
 *
 *	Description:
 */




#include "PCA.h"
#include <MMM/MMMCore.h>
#include <MMM/Motion/Legacy/LegacyMotion.h>
#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
#include <MMM/Motion/Legacy/LegacyMotionReaderC3D.h>
#include <MMM/Model/ModelProcessor.h>
#include <MMM/Model/ModelProcessorFactory.h>
#include <MMMSimoxTools/MMMSimoxTools.h>
#include <VirtualRobot/RobotNodeSet.h>
#include "gnuplot_i.hpp"
#include <vector>
#include <iostream>
#include <fstream>

double lengthSegment1;
double lengthSegment2;
std::vector<gsl::vector> elbowLeft;
std::vector<gsl::vector> elbowRight;
std::ofstream segmentsPCA;
std::ofstream segmentsVC;
//double alpha = 7.5;
//double beta = 14.5;//16


std::vector<gsl::vector> loadMotionFile(const char* dataFile, const char* object)
{
    std::cout << "Loading MMM data..." << std::endl;

    std::vector<gsl::vector> result;

std::map<std::string, VirtualRobot::RobotPtr> robots;
std::map<std::string, VirtualRobot::RobotNodeSetPtr> robotNodeSets;
MMM::LegacyMotionList motions;
    MMM::LegacyMotionReaderXMLPtr r(new MMM::LegacyMotionReaderXML());
    std::vector < std::string > motionNames = r->getMotionNames(dataFile);

    if (motionNames.size() == 0)
    {
        MMM_ERROR << "no motions in file " << std::endl;
        return result;
    }

    int frameCount = -1;
    for(size_t i = 0; i < motionNames.size(); i++)
    {
        MMM::LegacyMotionPtr motion = r->loadMotion(dataFile,motionNames[i]);
        if(!motion)
            continue;
        std::cout << "Loading motion: " << motion->getName() << std::endl;
        if(frameCount != -1 && frameCount != (int) motion->getNumFrames())
        {
            MMM_ERROR << "Error whole loading motions: the frame count between two motions do not match. All motions need to have the same framecount." << std::endl;
            return result;
        }
        else
            frameCount = motion->getNumFrames();

        std::cout  << "model file: " << motion->getModel()->getFilename() << std::endl;

        if ( motion->getModel())
        {
            robots[motion->getName()] = MMM::SimoxTools::buildModel( motion->getModel());
            MMM::SimoxTools::updateInertialMatricesFromModels(robots[motion->getName()]);
        }
        VirtualRobot::RobotPtr robot = robots[motion->getName()];

        if (motion)
        {
            std::vector<std::string> jointNames = motion->getJointNames();
            if (jointNames.size()>0)
            {
                std::string rnsName("MMMViewerRNS");
                if (robot->hasRobotNodeSet(rnsName))
                    robot->deregisterRobotNodeSet(robot->getRobotNodeSet(rnsName));
                robotNodeSets[motion->getName()] = VirtualRobot::RobotNodeSet::createRobotNodeSet(robot, rnsName, jointNames, "", "", true);
            }
        }
        
        motions.push_back(motion);
    }

    for (unsigned int i = 0; i < motions[0]->getNumFrames(); i++)
        {
	  gsl::vector entry(3);
  
      for (unsigned int j = 0; j < motions.size();j++)
	    {
	      if ((motions[j]->getName()).compare(object) == 0)
		{
		  MMM::MotionFramePtr md = motions[j]->getMotionFrame(i);
		  Eigen::Matrix4f tcpPose = md->getRootPose();
		  for (int j = 0; j < 3; j++){
		    entry(j) = tcpPose(j,3);

		  }
		}
	      /*else if ((motions[j]->getName()).compare("rightHand"))
		{
		  MMM::MotionFramePtr md = motions[j]->getMotionFrame(i);
		  Eigen::Matrix4f tcpPose = md->getRootPose();
		  for (int j = 0; j < 3; j++){
		    entry(3+j) = tcpPose(j,3);

		  }
		  }*/
	    }
	  result.push_back(entry);
	}
    std::cout << "Loading MMM data... done..." << std::endl;
    return result;
}

/*std::vector<gsl::vector> loadData(const char* filename)
{
  std::vector<gsl::vector> result;
  CC3DFileReader fileReader;
  if (fileReader.ReadDataFile(filename))
  {
	 std::vector<int> index;
	 std::vector<std::string> markerNamesToLookFor;
	 markerNamesToLookFor.push_back(std::string("UpperBody180213:RSHO"));
	 markerNamesToLookFor.push_back(std::string("UpperBody180213:RH7"));
	 markerNamesToLookFor.push_back(std::string("UpperBody180213:LSHO"));
	 markerNamesToLookFor.push_back(std::string("UpperBody180213:LH7"));
	 int elbowIndex = 0;
	int elbowIndex1 = 0; 
	 std::vector<std::string> markerNames = fileReader.GetMarkerNames();
	 for (int i = 0; i < markerNames.size(); i++)
	 	std::cout << markerNames[i].c_str() << std::endl;
	 for (int k = 0; k < markerNamesToLookFor.size(); k++){
	 for (int i = 0; i < markerNames.size(); i++)
	 {
		if (markerNames[i].compare(markerNamesToLookFor[k]) == 0) 
			index.push_back(i);
		else if (markerNames[i].compare("UpperBody180213:RFRA") == 0)
			elbowIndex = i; 
		else if (markerNames[i].compare("UpperBody180213:LFRA") == 0)
			elbowIndex1 = i; 

	}
}
  	std::vector<gsl::vector> rawData = fileReader.GetRawDataValues();
  	int offset = fileReader.GetOffset();
  
  	for (int i = 0; i < rawData.size(); i++)
  	{
	
		gsl::vector singleDataPoint(index.size()*3);
		gsl::vector singleElbowLeftPoint(3);
		gsl::vector singleElbowRightPoint(3);
		for (int j = 0; j < index.size(); j++)
		{
			
		singleDataPoint(j*3) = rawData[i](index[j]*3+offset);
		singleDataPoint(j*3+1) = rawData[i](index[j]*3+offset+1);
		singleDataPoint(j*3+2) = rawData[i](index[j]*3+offset+2);
			
		}
singleElbowLeftPoint(0) = rawData[i](elbowIndex1*3+offset);
		singleElbowLeftPoint(1) = rawData[i](elbowIndex1*3+offset+1);
		singleElbowLeftPoint(2) = rawData[i](elbowIndex1*3+offset+2);
		singleElbowRightPoint(0) = rawData[i](elbowIndex*3+offset);
		singleElbowRightPoint(1) = rawData[i](elbowIndex*3+offset+1);
		singleElbowRightPoint(2) = rawData[i](elbowIndex*3+offset+2);
	elbowRight.push_back(singleElbowRightPoint);
	elbowLeft.push_back(singleElbowLeftPoint);
	result.push_back(singleDataPoint);
	}
  	lengthSegment1 = 0.0;
  	lengthSegment2 = 0.0;
  	for (int i = 0; i < 3; i++)
  	{
		lengthSegment1 += (result[0](i)-rawData[0](elbowIndex*3+offset+i))*(result[0](i)-rawData[0](elbowIndex*3+offset+i));
  		lengthSegment2 += (result[0](3+i)-rawData[0](elbowIndex*3+offset+i))*(result[0](3+i)-rawData[0](elbowIndex*3+offset+i));
	}
	
  	lengthSegment1 = sqrt(lengthSegment1);
  	lengthSegment2 = sqrt(lengthSegment2);
	
}
  else
  	printf("Could not load data file.\n");
  return result;
}*/

Gnuplot plotData(std::vector<gsl::vector> data, const char* name)
{
  Gnuplot result;
  result.set_style("lines");
  for (unsigned int i = 0; i < data[0].size(); i++)
    {
      std::vector<double> plotValues;
      for (unsigned int j = 0; j < data.size(); j++)
	{
	  plotValues.push_back(data[j](i));
	}
      
      char buffer[50];
      sprintf(buffer," dim %d",i);
      std::string plotName(name);
      plotName += buffer;
      result.plot_x(plotValues,plotName.c_str());	
    }
  return result;
}

Gnuplot plotSegmentedData(std::vector<gsl::vector> data, const char* name)
{
  Gnuplot result;
  result.set_style("lines");
  double minX = 10000.0;
  double maxX = 0.0;
  for (unsigned int i = 0; i < data[0].size()-1; i++)
    {
      std::vector<double> plotValues;
      for (unsigned int j = 0; j < data.size(); j++)
	{
		if (minX > data[j](i))
			minX  = data[j](i);
		if (maxX < data[j](i))
			maxX  = data[j](i);
	  plotValues.push_back(data[j](i));
	}
      
      char buffer[50];
      sprintf(buffer," dim %d",i);
      std::string plotName(name);
      plotName += buffer;
      result.plot_x(plotValues,plotName.c_str());	

    }
   for (unsigned int i = 0; i < data.size(); i++)
     if (data[i](data[i].size()-1) == 1)
       {
	 std::vector<double> plotValues_x;
	 std::vector<double> plotValues_y;
	 //result.plot_x(i);
	 for (int j = -1000; j < 1000; j++)
	   {
	     if (j % 20 == 0)
	       {
	     plotValues_x.push_back(i);
	     
	     plotValues_y.push_back(j);
	       }
	   }

	 result.plot_xy(plotValues_x,plotValues_y);
       }
  
  return result;
}

double dotP(gsl::vector v1, gsl::vector v2) {
	double sum = 0;	
    for (unsigned int i = 0; i < v1.size(); i++) {
		sum += v1[i] * v2[i];
	}
	return sum;
}
std::vector<gsl::vector> convertToJointAngles(std::vector<gsl::vector> cartData)
{
	
	std::vector<gsl::vector> jointAngleData;
	
	
  for (unsigned int i = 0; i < cartData.size(); i++)
    {
      gsl::vector dataEntry(2);
      gsl::vector rightShoulderPosition(3);
      gsl::vector leftShoulderPosition(3);
      gsl::vector rightHandPosition(3);
      gsl::vector leftHandPosition(3);
      gsl::vector rightElbowPosition(3);
      gsl::vector leftElbowPosition(3);
   
      for (int j = 0; j < 3; j++)
      {
	rightShoulderPosition(j) = cartData[i][0+j];
	leftShoulderPosition(j) = cartData[i][6+j];
	rightHandPosition(j) = cartData[i][3+j];
	leftHandPosition(j) = cartData[i][9+j];
	rightElbowPosition(j) = elbowRight[i][j];
	leftElbowPosition(j) = elbowLeft[i][j];
      }

      /**************************                                     
   DONE
Compute joint angle right elbow
put in dataEntry(0)
      ****************************/
      gsl::vector angle_upper(3);
      gsl::vector angle_lower(3);
      for (int j = 0; j < 3; j++)
      {
		  angle_upper(j) = rightShoulderPosition(j) - rightElbowPosition(j);
		  angle_lower(j) = rightHandPosition(j) - rightElbowPosition(j);
	  }
	  double u_norm = sqrt(dotP(angle_upper, angle_upper));
	  double l_norm = sqrt(dotP(angle_lower, angle_lower));
	  dataEntry(0) = acos(dotP(angle_upper, angle_lower) / (u_norm * l_norm)) * 360 / (2 * M_PI);

	
      /**************************
   DONE
Compute joint angle left elbow
put in dataEntry(1)
      ****************************/
      for (int j = 0; j < 3; j++)
      {
		  angle_upper(j) = leftShoulderPosition(j) - leftElbowPosition(j);
		  angle_lower(j) = leftHandPosition(j) - leftElbowPosition(j);
	  }
	  u_norm = sqrt(dotP(angle_upper, angle_upper));
	  l_norm = sqrt(dotP(angle_lower, angle_lower));
	  dataEntry(1) = acos(dotP(angle_upper, angle_lower) / (u_norm * l_norm)) * 360 / (2 * M_PI);
	  
	  
      jointAngleData.push_back(dataEntry);
    }
    
  return jointAngleData;
}


std::vector<gsl::vector> segmentData_MethodA(std::vector<gsl::vector> orgData, double alpha)
{

  std::vector<gsl::vector> segmentedData;
  double sum;
  //int prevFlag = 0;
  int nSegPoints = 0;
  for (unsigned int i = 0; i < orgData.size(); i++)
    {
		sum = 0;
      gsl::vector dataEntryWithSegmentationFlag(orgData[i].size()+1);
      for (unsigned int j = 0; j < orgData[i].size(); j++)
		{
		dataEntryWithSegmentationFlag(j) = orgData[i](j);
		if (i > 0) sum += (orgData[i](j) - orgData[i-1](j)) * (orgData[i](j) - orgData[i-1](j));
		}
      dataEntryWithSegmentationFlag(orgData[i].size()) = 0;
      if (sum < alpha){
	nSegPoints++;
      }
      else
	{
	  if (nSegPoints > 0)
	    {
	      if (orgData[i].size()-int(nSegPoints/2) >= 0)
		{
          printf("current frame %d %d %d\n",(int)orgData[i].size(),int(nSegPoints/2),nSegPoints);
		  segmentedData[segmentedData.size()-1-int(nSegPoints/2)](orgData[i].size()) = 1;
		  segmentsVC << segmentedData.size()-1-int(nSegPoints/2) << std::endl;
		  //		  dataEntryWithSegmentationFlag(orgData[i].size()-int(nSegPoints/2)) = 1;
		  nSegPoints = 0;
		}
	      
	    }
	}

		
      /**************************
   DONE
   Set dataEntryWithSegmentationFlag(orgData[i].size()) = 0 if no segmentation point
Set dataEntryWithSegmentationFlag(orgData[i].size()) = 1 if segmentation point
      *****************************/
      
      
      
      segmentedData.push_back(dataEntryWithSegmentationFlag);
    }
  return segmentedData;
}
std::vector<gsl::vector> segmentData_MethodB(std::vector<gsl::vector> orgData, double alpha, int reducedDim)
{
	CPCA pca;
	int dim = reducedDim;
	std::vector<gsl::vector> segmentedData;
	std::vector<gsl::vector> reducedSet;
    for (unsigned int i = 0; i < orgData.size(); i++)
    {
      // double prevSum = 0;
      gsl::vector dataEntryWithSegmentationFlag(orgData[i].size()+1);
      for (unsigned int j = 0; j < orgData[i].size(); j++) {
		dataEntryWithSegmentationFlag(j) = orgData[i](j);
	  }
	
		reducedSet.push_back(orgData[i]);
	
	
      /**************************
   DO PROCESSING HERE
Use PCA.cpp
   Set dataEntryWithSegmentationFlag(orgData[i].size()) = 0 if no segmentation point
Set dataEntryWithSegmentationFlag(orgData[i].size()) = 1 if segmentation point

      *****************************/
      
      
      bool segment = false;
      
      if (reducedSet.size() >= 2) {
		pca.computePCA_Cov(reducedSet, dim);
		std::vector<gsl::vector> restored = pca.restoreDim(pca.reduceDim(reducedSet));      
      
      // compare segmentedData with restored
      double sum = 0;
     double singleSum = 0;
      for (unsigned int n = 0; n < restored.size() && !segment; n++) {
		  singleSum = 0;
          for (unsigned int j = 0; j < restored[0].size(); j++) {
			  singleSum += (restored[n](j)-reducedSet[n](j)) * (restored[n](j)-reducedSet[n](j));
		  }
		  //		  std::cout << sum << std::endl;
		  singleSum = sqrt(singleSum);
		  sum += singleSum;
		  
	  }
      sum = sum/(reducedSet.size()-1);
      sum = (sum+singleSum)/2.0;
      if (sum > alpha) {
			  dataEntryWithSegmentationFlag(orgData[i].size()) = 1;
			  segmentsPCA << i << std::endl;
			  segment = true;
			  reducedSet.clear();
		  }
      // prevSum = sum;
      
      }
      if (!segment) {
		  dataEntryWithSegmentationFlag(orgData[i].size()) = 0;
	  }
	  
	  segmentedData.push_back(dataEntryWithSegmentationFlag);
      
      
    }
  return segmentedData;
}

int main(int argc, char **args)
{
	if (argc != 3)
	{
		printf("Usage: MMMSegmenter /path/to/file object\n");
		return 1;
	}
	std::string objectName(args[2]);
	std::string segmentsPCAfile(args[1]);
	std::string segmentsVCfile(args[1]);
	segmentsPCAfile += "_PCA_seg_";
	segmentsVCfile += "_VC_seg_";
	segmentsPCAfile += objectName;
	segmentsVCfile += objectName;
	segmentsPCA.open(segmentsPCAfile.c_str());
	segmentsVC.open(segmentsVCfile.c_str());
	std::vector<gsl::vector> testData = loadMotionFile(args[1], args[2]);
	std::vector<gsl::vector> reducedTestData(testData.size() - 60);
	
    for (unsigned int i = 60; i < testData.size(); i++) {
		reducedTestData[i - 60] = testData[i];
	}
	
	//testData = reducedTestData;

	std::vector<gsl::vector> dataOriginalCartesian =  testData;	
    //std::vector<gsl::vector> dataOriginalJointAngles =  convertToJointAngles(testData);
	std::vector<gsl::vector> dataSegmentedCartesian_MethodA =  segmentData_MethodA(testData, 0.05);
	std::vector<gsl::vector> dataSegmentedCartesian_MethodB =  segmentData_MethodB(testData, 30.5, 1);
    //std::vector<gsl::vector> dataSegmentedJointAngles_MethodA =  segmentData_MethodA(dataOriginalJointAngles, .001);
    //std::vector<gsl::vector> dataSegmentedJointAngles_MethodB =  segmentData_MethodB(dataOriginalJointAngles, 10000, 1);
	segmentsPCA.close();
	segmentsVC.close();
	//COMMENT: Only max 2 plots at the same time can be displayed
	//Gnuplot plotOriginalCartesian = plotData(dataOriginalCartesian,"original");
	//UNCOMMENT to display joint angle results
	//Gnuplot plotOriginalJointAngles = plotData(dataOriginalJointAngles,"joint angles");
	//UNCOMMENT to display segmentation results in Cartesian Space with method A
    Gnuplot plotSegmentedCartesian_MethodA = plotSegmentedData(dataSegmentedCartesian_MethodA,"Velocity crossings");
	//UNCOMMENT to display segmentation results in Cartesian Space with method B
   	Gnuplot plotSegmentedCartesian_MethodB = plotSegmentedData(dataSegmentedCartesian_MethodB,"PCA");	
	//UNCOMMENT to display segmentation results in Joint Space with method A
    //Gnuplot plotSegmentedJointAngles_MethodA = plotSegmentedData(dataSegmentedJointAngles_MethodA,"joint method a");
	//UNCOMMENT to display segmentation results in Joint Space with method B
    //Gnuplot plotSegmentedJointAngles_MethodB = plotSegmentedData(dataSegmentedJointAngles_MethodB,"joint method b");
	while(true){}
	return 0;
}



